﻿/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include "SerialTools.h"
#include <QDebug>

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
** Implementation
*****************************************************************************/

RobotCmdNode::RobotCmdNode(SerialTools* sp)
    : Node("serialtools_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), m_sp(sp)
{
    // m_pubCmd = this->create_publisher<serialtool_msg::msg::SerialTool>("pccmd",1);

    subscription_ = this->create_subscription<serialtool_msg::msg::SerialTool>(
        "SerialTool", 1, std::bind(&RobotCmdNode::topic_callback, this, _1));

    // getParameters();
    RCLCPP_INFO(this->get_logger(), "SerialTools_node...");
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}

void RobotCmdNode::topic_callback(const serialtool_msg::msg::SerialTool::SharedPtr msg) const
{
    qDebug() << "serial toos node pccmd callback//////////////////";
    RCLCPP_INFO(this->get_logger(), "/////// I heard cmd: '%s', content: '%s'", msg->cmdname.c_str(), msg->cmdcontent.c_str());

    if (msg->cmdname != "serialtools") {
        return;
    }

    RCLCPP_INFO(this->get_logger(), "I heard cmd: '%s', content: '%s'", msg->cmdname.c_str(), msg->cmdcontent.c_str());

    m_sp->call_cmd(msg->cmdcontent);
}

void RobotCmdNode::getParameters()
{
    RCLCPP_INFO(this->get_logger(), "getParameters...");

    get_parameter_or(std::string("com_name"), m_strComName, std::string("no"));

    RCLCPP_INFO(this->get_logger(), "com_name: %s", m_strComName.c_str());

    m_sp->setCom(m_strComName);
}
